#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1

// 引脚定义
#define POTENTIOMETER_PIN 33
#define X_PIN 34
#define Y_PIN 35

// 油门引脚
#define SWITCH_PIN 4

#define OLED_SCL 17
#define OLED_SDA 16

const int gear_back = -1;
const int gear_advance = 1;
const int gear_super_advance = 2;

// 电机1引脚定义
const int RPWM_PIN = 18;   // 连接BTS7960 RPWM
const int LPWM_PIN = 19;   // 连接BTS7960 LPWM
const int ENABLE_PIN = 5;  // 连接R_EN和L_EN

// 电机2引脚定义
const int RPWM_PIN2 = 12;  // 连接BTS7960 RPWM
const int LPWM_PIN2 = 13;  // 连接BTS7960 LPWM
const int ENABLE_PIN2 = 2; // 连接R_EN和L_EN

// PWM参数
const int PWM_FREQ = 5000;      // PWM频率(Hz)
const int PWM_RESOLUTION = 8;   // 8位分辨率(0-255)
const int grad = 20;            // 电机加速度

int currentLeftSpeed = 0;
int currentRightSpeed = 0;
int speed = 0;

// 左转右转参数
#define slowspeed 50
#define quickspeed 150

// 定义电机通道结构体
struct MotorChannels {
  int channel_r;
  int channel_l;
};

// 创建电机通道配置
MotorChannels leftMotor = {0, 1};   // 左侧电机使用通道0和1
MotorChannels rightMotor = {2, 3};  // 右侧电机使用通道2和3

// 摇杆校准参数
int centerX = 1900;        // 中心点X值
int centerY = 1840;        // 中心点Y值
const int deadZone = 300;  // 调整为更合理的死区值

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// ========== 函数前置声明 ==========
void setMotorSpeed(int speed, char motor);
void stopMotor(char motor);
int gear_state(float voltage, int gear);
int approachZero(int speed);

void setup() {
  delay(1000);
  Serial.begin(115200);
  // 初始化开关
  // 强制启用上拉（即使引脚有问题）
  pinMode(SWITCH_PIN, OUTPUT);
  digitalWrite(SWITCH_PIN, HIGH);  // 开启上拉
  pinMode(SWITCH_PIN, INPUT);      // 切换回输入模式但保持上拉

  // 初始化I2C
  Wire.begin(OLED_SDA, OLED_SCL);

  // 初始化OLED
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    while (1);  // 卡死
  }

  // 显示欢迎信息
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 10);
  display.println(F("Joystick Monitor"));
  display.println(F("Initialized..."));
  display.display();
  delay(1000);  // 显示1秒

  // 初始化使能引脚
  pinMode(ENABLE_PIN, OUTPUT);
  digitalWrite(ENABLE_PIN, HIGH);  // 启用驱动芯片
  pinMode(ENABLE_PIN2, OUTPUT);
  digitalWrite(ENABLE_PIN2, HIGH);  // 启用驱动芯片

  // 配置PWM通道
  ledcSetup(leftMotor.channel_r, PWM_FREQ, PWM_RESOLUTION);
  ledcSetup(leftMotor.channel_l, PWM_FREQ, PWM_RESOLUTION);
  ledcSetup(rightMotor.channel_r, PWM_FREQ, PWM_RESOLUTION);
  ledcSetup(rightMotor.channel_l, PWM_FREQ, PWM_RESOLUTION);

  // 绑定PWM通道到GPIO
  ledcAttachPin(RPWM_PIN, leftMotor.channel_r);
  ledcAttachPin(LPWM_PIN, leftMotor.channel_l);
  ledcAttachPin(RPWM_PIN2, rightMotor.channel_r);
  ledcAttachPin(LPWM_PIN2, rightMotor.channel_l);

  // 初始停止电机
  stopMotor('l');
  stopMotor('r');
}

void loop() {
  // 读取传感器数据
  int rawX = analogRead(X_PIN) - centerX;
  int rawY = analogRead(Y_PIN) - centerY;
  int sensorValue = analogRead(POTENTIOMETER_PIN);
  int gear = 0;
  // 计算电压值 (0-3.3V)
  float voltage = sensorValue * (3.3 / 4095.0);
  // 开关定义
  int switchState = digitalRead(SWITCH_PIN);

  // 防抖
  if (abs(rawX) < deadZone) rawX = 0;
  if (abs(rawY) < deadZone) rawY = 0;
  gear = gear_state(voltage, gear);

  // 将xy映射到-255--255
  int resulty;
  if (rawY <= 0) {
    // 处理负数及0：[-1840,0] → [-255,0]
    resulty = map(rawY, -1840, 0, -255, 0);
  } else {
    // 处理正数：[0,2255] → [0,255]
    resulty = map(rawY, 0, 2255, 0, 255);
  }
  rawY = -resulty;
  int resultx;
  if (rawX <= 0) {
    // 处理负数及0：[-1840,0] → [-255,0]
    resultx = map(rawX, -1900, 0, -255, 0);
  } else {
    // 处理正数：[0,2255] → [0,255]
    resultx = map(rawX, 0, 2195, 0, 255);
  }
  rawX = resultx;

  // 计算目标速度（基于摇杆和挡位）
  int targetSpeed = 0;
  if (switchState == 1) {
    // 挡位仅限制最大速度（不控制方向）
    int maxSpeed;
    switch (gear) {
      case gear_back: maxSpeed = 150; break;          // 后退限速
      case gear_advance: maxSpeed = 200; break;       // 普通前进
      case gear_super_advance: maxSpeed = 255; break; // 全速前进
      default: maxSpeed = 0;
    }

    // 摇杆Y轴控制主体速度（反向映射）
    targetSpeed = constrain(-rawY, -maxSpeed, maxSpeed);
    
    // 摇杆X轴控制转向差速
    int turnOffset = map(rawX, -255, 255, -100, 100);
    int leftTarget = targetSpeed + turnOffset;
    int rightTarget = targetSpeed - turnOffset;
    
    // 渐进调整左电机速度
    if (currentLeftSpeed < leftTarget)
      currentLeftSpeed = min(currentLeftSpeed + grad, leftTarget);
    else if (currentLeftSpeed > leftTarget)
      currentLeftSpeed = max(currentLeftSpeed - grad, leftTarget);
      
    // 渐进调整右电机速度
    if (currentRightSpeed < rightTarget)
      currentRightSpeed = min(currentRightSpeed + grad, rightTarget);
    else if (currentRightSpeed > rightTarget)
      currentRightSpeed = max(currentRightSpeed - grad, rightTarget);
  } else {
    // 油门关闭时平滑停止
    currentLeftSpeed = approachZero(currentLeftSpeed);
    currentRightSpeed = approachZero(currentRightSpeed);
  }

  // 更新电机速度
  setMotorSpeed(currentLeftSpeed, 'l');
  setMotorSpeed(currentRightSpeed, 'r');
  
  // 更新速度显示值
  speed = (abs(currentLeftSpeed) + abs(currentRightSpeed)) / 2;

  // 清除并准备显示
  display.clearDisplay();
  display.setTextSize(1);
  display.setCursor(0, 0);
  
  // 显示油门状态
  if (switchState == 1) {
    display.println(F("Throttle: ON"));
  } else {
    display.println(F("Throttle: OFF"));
  }
  
  // 显示电位器数据
  display.print(F("Gear: "));
  display.print(gear);
  switch (gear) {
    case gear_back: display.println(", Back!"); break;
    case gear_advance: display.println(", Advance!"); break;
    case gear_super_advance: display.println(", SUPER ADVANCE!"); break;
  }
  
  display.print(F("Speed: "));
  display.println(speed);
  
  display.print(F("rawY: "));
  display.println(rawY);
  
  display.print(F("rawX: "));
  display.println(rawX);

  // 更新显示
  display.display();

  delay(100);  // 刷新率100ms
}

void setMotorSpeed(int speed, char motor) {
  MotorChannels channels;
  
  // 选择电机
  if (motor == 'l') {  // 使用==进行比较
    channels = leftMotor;
  } else if (motor == 'r') {
    channels = rightMotor;
  } else {
    return;  // 无效电机标识
  }

  if (speed > 0) {
    // 正转
    ledcWrite(channels.channel_r, speed);
    ledcWrite(channels.channel_l, 0);
  } else if (speed < 0) {
    // 反转
    ledcWrite(channels.channel_r, 0);
    ledcWrite(channels.channel_l, -speed);
  } else {
    // 停止
    ledcWrite(channels.channel_r, 0);
    ledcWrite(channels.channel_l, 0);
  }
}

// 停止特定电机
void stopMotor(char motor) {
  setMotorSpeed(0, motor);
}

int gear_state(float voltage, int gear) {
  // 挡位控制
  if (voltage < 1.1) 
    gear = gear_back;
  else if (voltage >= 1.1 && voltage < 2.2) 
    gear = gear_advance;
  else if (voltage >= 2.2 && voltage <= 3.4) 
    gear = gear_super_advance;
  
  return gear;
}

int approachZero(int speed) {
  if (speed > 0) 
    return max(0, speed - grad);
  if (speed < 0) 
    return min(0, speed + grad);
  return 0;
}